//== SAGITTARIUS ============================================================================

//== BEGINNING OF CODE ===============================================================

#include "sagittarius/core/SagittariusCore.h"
#include "sagittarius/parallel/atomics.h"
#include "sagittarius/mem/span.h"
#include "sagittarius/io/io.h"

//small object allocation
//TODO: put this entire thing in asm
void* smo_allocs_b(sg_span* s) {
    void *obj;
    freenode_t n, q, next, *pnext;
    size_t bs = s->bs;
    size_t headeroff = bs - 2;
    while(1) {
        n = s->freenode;
        if(n == SG_FREENODE_EXH)
            return NULL;
        pnext = (freenode_t*)((uint8*)s->addr + n*bs + headeroff); //TODO: lea
        next = *pnext;
        if(n != s->freenode)
            continue; //validate head & next are consistent
        if(next & 0x8000)
            //try to advance head here instead of block?
            continue; //BLOCKING object is owned by another process
        q = s->instnode;
        if(n == q) {
            sg_cas16(&s->instnode, q, next); //tail falling behind
            continue;
        }
    #if SG_SPAN_FREENODE_SIZE == 2
        if(sg_cas16(pnext, next, next|0x8000)) {
            //successfully popped node
            sg_cas16(&s->freenode, n, next); //try to swing head
            break;
        }
    }
    obj = (freenode_t*)s->addr + n;
    *((uint8*)obj+s->ob) = 0x80; //distinguishes alloc'd objs from freed ones
    #else
        #error "Unknown span freenode type"
    #endif
    return obj;
}

int smo_frees(sg_span* s, void* obj) {
    //TODO: init memory to bit pattern before calling this func
    //so we can prevent inappropriate/duplicate frees (sort of done)
    freenode_t n, q, next, *pnext;
    intptr_t boff = (uint8*)obj - (uint8*)s->addr;
    size_t bs = s->bs;
    freenode_t off = (freenode_t)(boff / bs);
    size_t headeroff = bs - 2;
    
    AZ(boff % s->bs, "Offset misaligned");
    
    AN(*(freenode_t*)((uint8*)obj+headeroff) & 0x8000, "Free applied to nonowned memory. Possible double use of free"); //TODO: return error
    //TODO: this makes the link live again - wait until after it's linked?
    *(freenode_t*)((uint8*)obj+headeroff) = 0x0000; //distinguishes alloc'd objs from freed ones
    
    //put header in a valid state
    *((freenode_t*)obj) = SG_FREENODE_EXH;
    while(1) {
        n = s->freenode;
        q = s->instnode;
        
        //startup from empty queue
        if(q == SG_FREENODE_EXH) {
            if(n != SG_FREENODE_EXH)
                continue;
            if(sg_cas16(&s->freenode, n, off)) { //start anew
                sg_cas16(&s->instnode, q, off); //try to swing tail
                break;
            }
        }
        
        //usual case
        pnext = (freenode_t*)((uint8*)s->addr + q*bs + headeroff); //TODO: lea
        next = *pnext; //ACCESS HAZARD (value may be invalid, but no segfault)
        
        if(q == s->instnode) { //validate tail & next are consistent
            if(next == SG_FREENODE_EXH) {
                if(sg_cas16(pnext, next, off)) { //FIXME: ACCESS HAZARD
                    sg_cas16(&s->instnode, q, off);//try to swing tail
                    break;
                }
            } else {
                sg_cas16(&s->instnode, q, next);//try to swing tail
            }
        }
    }
    return 0;
}

#if 0
int smo_push_direct(sg_span* s, void* obj) {
    //assumes that the object has already been prepared
    freenode_t n, q, next, *pnext;
    intptr_t boff = (uint8*)obj - (uint8*)s->addr;
    size_t bs = s->bs;
    freenode_t off = (freenode_t)(boff / bs);
    
    AZ(boff % s->bs, "Offset misaligned");
    
    //do not change the object header - it's supposed to be prepared by caller
    
    while(1) {
        n = s->freenode;
        q = s->instnode;
        
        if(q == SG_FREENODE_EXH) {
            if(n != SG_FREENODE_EXH)
                continue;
            if(sg_cas16(&s->freenode, n, off)) { //start anew
                sg_cas16(&s->instnode, q, off); //try to swing tail
                break;
            }
        }
        
        //usual case
        pnext = (freenode_t*)((uint8*)s->addr + q*bs); //TODO: lea
        next = *pnext;
        //Do we need to check that q is still tail here?
        //Yes, otherwise we may not assume ownership of the tail.
        if(q == s->instnode) {
            if(next == SG_FREENODE_EXH) {
                if(sg_cas16(pnext, next, off)) {
                    sg_cas16(&s->instnode, q, off);//try to swing tail
                    break;
                }
            } else {
                sg_cas16(&s->instnode, q, next);//try to swing tail
            }
        }
    }
    return 0;
}
#endif